﻿namespace MyTRCP.Common.Codecs.Sbc
{
    public class SbcEncoder
    {
        private readonly SbcConfig _config;
        private readonly SbcEncoderState _state;
        private bool _init;
        private readonly SbcFrame _frame;
        public SbcEncoder(bool isBigEndian)
        {
            _config = isBigEndian ? SbcConfig.CreateMyVoiceSbcConfigBE() : SbcConfig.CreateMyVoiceSbcConfigLE();
            _state = new SbcEncoderState();
            _frame = new SbcFrame();
        }
        private void SbcEncoderInit()
        {
            _state.X = new short[2, SbcCommon.SBC_X_BUFFER_SIZE];
            _state.Position = SbcCommon.SBC_X_BUFFER_SIZE - _frame.Subbands * 9 & ~7;
            _state.Increment = 4;
        }
        private int SbcAnalyzeAudio()
        {
            int ch, blk;
            int xPos;

            switch (_frame.Subbands)
            {
                case 4:
                    for (ch = 0; ch < _frame.Channels; ch++)
                    {
                        xPos = _state.Position - 4 * _state.Increment + _frame.Blocks * 4;
                        for (blk = 0; blk < _frame.Blocks; blk += _state.Increment)
                        {
                            _state.SbcAnalyze4s(_state.X, ch, xPos, _frame.SbSampleF, blk, ch);
                            xPos -= 4 * _state.Increment;
                        }
                    }
                    return _frame.Blocks * 4;
                case 8:
                    for (ch = 0; ch < _frame.Channels; ch++)
                    {
                        xPos = _state.Position - 8 * _state.Increment + _frame.Blocks * 8;
                        for (blk = 0; blk < _frame.Blocks; blk += _state.Increment)
                        {
                            _state.SbcAnalyze8s(_state.X, ch, xPos, _frame.SbSampleF, blk, ch);
                            xPos -= 8 * _state.Increment;
                        }
                    }
                    return _frame.Blocks * 8;
                default:
                    return -5;
            }
        }
        public int SbcEncode(byte[] input, int inputPos, int input_len, byte[] output, int outputPos, int output_len, ref int written)
        {
            int samples;
            int framelen;
            SbcEncoderState.SbcEncProcessInputXsXe sbc_enc_process_input;

            if (_config == null || input == null)
                return -5;

            written = 0;

            if (!_init)
            {
                _frame.Frequency = _config.Frequency;
                _frame.Mode = _config.Mode;
                _frame.Channels = (byte)(_config.Mode == ChannelModeEnum.SBC_MODE_MONO ? 1 : 2);
                _frame.Allocation = _config.AllocationMethod;
                _frame.SubbandMode = _config.SubBandNum;
                _frame.Subbands = (byte)(_config.SubBandNum == SubbandsEnum.SBC_SB_8 ? 8 : 4);
                _frame.BlockMode = _config.BlockNum;
                _frame.Blocks = (byte)(4 + (byte)_config.BlockNum * 4);
                _frame.Bitpool = _config.Bitpool;
                _frame.Codesize = SbcCommon.SbcGetCodesize(_config, _init, _frame);
                _frame.Length = SbcCommon.SbcGetFrameLength(_config, _init, _frame);
                SbcEncoderInit();
                _init = true;
            }
            else if (_frame.Bitpool != _config.Bitpool)
            {
                _frame.Length = SbcCommon.SbcGetFrameLength(_config, _init, _frame);
                _frame.Bitpool = _config.Bitpool;
            }

            /* input must be large enough to encode a complete frame */
            if (input_len - inputPos < _frame.Codesize)
                return 0;

            /* output must be large enough to receive the encoded frame */
            if (output == null || output_len - outputPos < _frame.Length)
                return -28;

            /* Select the needed input data processing function and call it */
            if (_frame.Subbands == 8)
            {
                if (_config.DataEndianess == DataEndianessEnum.SBC_BE)
                    sbc_enc_process_input = _state.SbcEncProcessInput8sBe;
                else
                    sbc_enc_process_input = _state.SbcEncProcessInput8sLe;
            }
            else
            {
                if (_config.DataEndianess == DataEndianessEnum.SBC_BE)
                    sbc_enc_process_input = _state.SbcEncProcessInput4sBe;
                else
                    sbc_enc_process_input = _state.SbcEncProcessInput4sLe;
            }

            _state.Position = sbc_enc_process_input(_state.Position, input, inputPos, _state.X, _frame.Subbands * _frame.Blocks, _frame.Channels);

            samples = SbcAnalyzeAudio();

            if (_frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO)
            {
                int j = _state.SbcCalcScalefactorsJ(_frame.SbSampleF, _frame.ScaleFactor, _frame.Blocks, _frame.Subbands);
                framelen = SbcPackFrame(output, outputPos, output_len, j);
            }
            else
            {
                _state.SbcCalcScalefactors(_frame.SbSampleF, _frame.ScaleFactor, _frame.Blocks, _frame.Channels, _frame.Subbands);
                framelen = SbcPackFrame(output, outputPos, output_len, 0);
            }

            written = framelen;

            return samples * _frame.Channels * 2;
        }
        private int SbcPackFrame(byte[] data, int pos, int len, int joint)
        {
            int frame_subbands = 4;

            data[pos + 0] = SbcCommon.SBC_SYNCWORD;

            data[pos + 1] = (byte)(((byte)_frame.Frequency & 0x03) << 6);
            data[pos + 1] |= (byte)(((byte)_frame.BlockMode & 0x03) << 4);
            data[pos + 1] |= (byte)(((byte)_frame.Mode & 0x03) << 2);
            data[pos + 1] |= (byte)(((byte)_frame.Allocation & 0x01) << 1);

            data[pos + 2] = _frame.Bitpool;

            if (_frame.Subbands != 4)
                frame_subbands = 8;

            if ((_frame.Mode == ChannelModeEnum.SBC_MODE_MONO || _frame.Mode == ChannelModeEnum.SBC_MODE_DUAL_CHANNEL) && _frame.Bitpool > frame_subbands << 4)
                return -5;

            if ((_frame.Mode == ChannelModeEnum.SBC_MODE_STEREO || _frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO) && _frame.Bitpool > frame_subbands << 5)
                return -5;

            if (_frame.Subbands == 4)
            {
                if (_frame.Channels == 1)
                    return SbcPackFrameInternal(data, pos, len, 4, 1, joint);
                else
                    return SbcPackFrameInternal(data, pos, len, 4, 2, joint);
            }
            else
            {
                data[pos + 1] |= 0x01;
                if (_frame.Channels == 1)
                    return SbcPackFrameInternal(data, pos, len, 8, 1, joint);
                else
                    return SbcPackFrameInternal(data, pos, len, 8, 2, joint);
            }
        }
        private int SbcPackFrameInternal(byte[] data, int basePos, int len, int frame_subbands, int frame_channels, int joint)
        {
            void PUT_BITS(byte[] data, int basePos, ref int pos, ref uint bits_cache, ref uint bits_count, int v, int n)
            {
                bits_cache = (uint)v | bits_cache << n;
                bits_count += (uint)n;
                if (bits_count >= 16)
                {
                    bits_count -= 8;
                    data[basePos + pos++] = (byte)(bits_cache >> (int)bits_count);
                    bits_count -= 8;
                    data[basePos + pos++] = (byte)(bits_cache >> (int)bits_count);
                }
            }
            void FLUSH_BITS(byte[] data, int basePos, ref int pos, ref uint bits_cache, ref uint bits_count)
            {
                while (bits_count >= 8)
                {
                    bits_count -= 8;
                    data[basePos + pos++] = (byte)(bits_cache >> (int)bits_count);
                }
                if (bits_count > 0)
                {
                    data[basePos + pos++] = (byte)(bits_cache << (int)(8 - bits_count));
                }
            }
            /* Bitstream writer starts from the fourth byte */
            int pos = 4;
            uint bits_cache = 0;
            uint bits_count = 0;

            /* Will copy the header parts for CRC-8 calculation here */
            byte[] crc_header = new byte[11];
            int crc_pos;

            uint audio_sample;

            int ch, sb, blk;    /* channel, subband, block and bit counters */
            int[,] bits = new int[2, 8];     /* bits distribution */
            uint[,] levels = new uint[2, 8];  /* levels are derived from that */
            uint[,] sb_sample_delta = new uint[2, 8];

            /* Can't fill in crc yet */

            crc_header[0] = data[basePos + 1];
            crc_header[1] = data[basePos + 2];
            crc_pos = 16;

            if (_frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO)
            {
                PUT_BITS(data, basePos, ref pos, ref bits_cache, ref bits_count, joint, frame_subbands);
                crc_header[crc_pos >> 3] = (byte)joint;
                crc_pos += frame_subbands;
            }

            for (ch = 0; ch < frame_channels; ch++)
            {
                for (sb = 0; sb < frame_subbands; sb++)
                {
                    PUT_BITS(data, basePos, ref pos, ref bits_cache, ref bits_count, (int)(_frame.ScaleFactor[ch, sb] & 0x0F), 4);
                    crc_header[crc_pos >> 3] <<= 4;
                    crc_header[crc_pos >> 3] |= (byte)(_frame.ScaleFactor[ch, sb] & 0x0F);
                    crc_pos += 4;
                }
            }

            /* align the last crc byte */
            if (crc_pos % 8 != 0)
                crc_header[crc_pos >> 3] <<= 8 - crc_pos % 8;

            data[basePos + 3] = SbcCommon.SbcCrc8(crc_header, crc_pos);

            SbcCommon.SbcCalculateBits(_frame, bits);

            for (ch = 0; ch < frame_channels; ch++)
            {
                for (sb = 0; sb < frame_subbands; sb++)
                {
                    levels[ch, sb] = (uint)((1 << bits[ch, sb]) - 1 << 32 - (int)(_frame.ScaleFactor[ch, sb] + SbcCommon.SCALE_OUT_BITS + 2));
                    sb_sample_delta[ch, sb] = (uint)1 << (int)(_frame.ScaleFactor[ch, sb] + SbcCommon.SCALE_OUT_BITS + 1);
                }
            }

            for (blk = 0; blk < _frame.Blocks; blk++)
            {
                for (ch = 0; ch < frame_channels; ch++)
                {
                    for (sb = 0; sb < frame_subbands; sb++)
                    {

                        if (bits[ch, sb] == 0)
                            continue;

                        audio_sample = (uint)(levels[ch, sb] * (ulong)(sb_sample_delta[ch, sb] + _frame.SbSampleF[blk, ch, sb]) >> 32);

                        PUT_BITS(data, basePos, ref pos, ref bits_cache, ref bits_count, (int)audio_sample, bits[ch, sb]);
                    }
                }
            }

            FLUSH_BITS(data, basePos, ref pos, ref bits_cache, ref bits_count);

            return pos;
        }
    }
}
